#ifndef ROBORTS_DECISION_BLACKBOARD_H
#define ROBORTS_DECISION_BLACKBOARD_H
#include <actionlib/client/simple_action_client.h>
#include<tf/tf.h>
#include<tf/transform_listener.h>
#include<ros/ros.h>
#include<geometry_msgs/PoseStamped.h>

#include "roborts_msgs/ArmorDetectionAction.h"
#include "roborts_msgs/GameResult.h"
#include "roborts_msgs/GameStatus.h"
#include "roborts_msgs/RobotStatus.h"
#include "roborts_msgs/RobotDamage.h"
#include "roborts_msgs/RobotBonus.h"
#include "roborts_msgs/BonusStatus.h"
#include "roborts_msgs/ProjectileSupply.h"
#include "roborts_msgs/SupplierStatus.h"
#include "roborts_msgs/ShootInfo.h"
#include "roborts_msgs/GimbalMode.h"
#include "roborts_msgs/ShootCmd.h"
#include "roborts_msgs/FricWhl.h"
#include "roborts_msgs/GameSurvivor.h"
#include "roborts_msgs/SentryInfo.h"
#include "roborts_msgs/PunishInfo.h"
#include "roborts_msgs/RobotInfo.h"
#include "roborts_msgs/TreeStatus.h"
#include "roborts_msgs/GimbalControl.h"

//#include "roborts_msgs/SwingDefend.h"
#include "roborts_msgs/PyArmorInfo.h"

#include "io/io.h"
#include "../proto/decision.pb.h"
#include "costmap/costmap_interface.h"

#include "../executor/chassis_executor.h"
#include "../executor/gimbal_executor.h"
#include "sdk.h"
#include "gimbal.h"
#include "topic_name.h"
//#include "pos_reciever.cpp"


namespace roborts_decision{
  enum class GimbalMode{
      GYRO_CONTROL = 0,
      CODE_CONTROL = 1,
      G_MODE_MAX_NUM = 2,
  };

  enum class ShootMode{
      SHOOT_STOP = 0,
      SHOOT_ONCE = 1,
      SHOOT_CONTINUOUS = 2,
  };

  enum _Status{
    CHASE   = 1,
    ESCAPE  = 2,
    SHOOT   = 3,
    BUFF    = 4
};

  class Blackboard{
    
    public:

      typedef std::shared_ptr<Blackboard> Ptr;
      typedef roborts_costmap::CostmapInterface CostMap;
      typedef roborts_costmap::Costmap2D CostMap2D;
      explicit Blackboard(const std::string &proto_file_path):
        is_follow_(false),
        has_got_(false),
        red_bonus_(0), // 
        blue_bonus_(0), //
        red_supplier_(0), //
        blue_supplier_(0), //
        id_(3), //
        is_master_(true),
        action_state_(BehaviorState::IDLE), //      
        //chase_count_(0),
        is_chase_(false),
        //shoot_mode_(0),
        //shoot_number_(0),
        remaining_time_(180),            
        enemy_lost_(false),
        sentry_lost_(false),
        cancel_flag_(true),
        is_scan_(false),
        is_swing_(false),
        search_count_(0),
        search_points_size_(0),
        is_escape_(false),                                             
        escape_yaw_(0),
        damage_source_(0), //
        damage_source_last_(0), //
        damage_source_now_(0), //
        damage_type_(0), //
        mate_attacked_(false),
        last_armor_attack_time_(ros::Time::now()),
        last_check_attacked_time_(ros::Time::now()),
        last_check_attack_time_(ros::Time::now()),
        remain_bullet_(0),
        remain_hp_(2000),
        enemy_hp_(2000),
        last_hp_(2000),
        last_enemy_hp_(2000),
        self_dmp_(0),
        enemy_dmp_(0),
        if_punished_(false),
        mate_punished_(false),         
        gimbal_punished_(false),
        chassis_punished_(false),
        mate_gimbal_(false),
        mate_chassis_(false),
        teammate_hp_(2000),
        teammate_bullet_(0),
        teammate_heat_(0),
        heat_(0),
        mate_tree_running_(false),
        camera_lost_(false),
        camera_dis_(0),
        game_status_(0),
        near_enemy_num_(0),
        chase_enemy_num_(0)
      {
        

        tf_ptr_ = std::make_shared<tf::TransformListener>(ros::Duration(10));
        std::string map_path = ros::package::getPath("roborts_costmap") + \
        "/config/costmap_parameter_config_for_decision.prototxt";
        costmap_ptr_ = std::make_shared<CostMap>("decision_costmap", *tf_ptr_,
                                             map_path);
        chassis_executor_ptr_=std::make_shared<ChassisExecutor>();
        charmap_ = costmap_ptr_->GetCostMap()->GetCharMap();
        costmap_2d_ = costmap_ptr_->GetLayeredCostmap()->GetCostMap();
      
        //gimbal control
        ros::NodeHandle control_nh;
        gim_control_pub = control_nh.advertise<roborts_msgs::GimbalControl>("GImbalControl",30);

        //interact info
        ros::NodeHandle interact_nh;   
        mate_info_sub = interact_nh.subscribe<roborts_msgs::RobotInfo>("robort_info",30, &Blackboard::MateInfoCallBack, this);
        mate_punish_sub = interact_nh.subscribe<roborts_msgs::PunishInfo>("robort_info",30, &Blackboard::MatePubnishCallBack, this);
        mate_treestatus_sub = interact_nh.subscribe<roborts_msgs::TreeStatus>("robort_info",30, &Blackboard::MateTreeStatusCallBack, this);
      
        //sentry info
        ros::NodeHandle sen_nh;
        enemy_pose_sub = sen_nh.subscribe<roborts_msgs::SentryInfo>("enemy_pose_from_sentry",30, &Blackboard::EnemyPoseCallBack, this);
     
        //camera info
        //ros::NodeHandle camera_nh;
        //camera_sub = camera_nh.subscribe<roborts_msgs::PyArmorInfo>("PyArmorInfo",30, &Blackboard::CameraCallBack,this);

        chase_goal_[0].header.frame_id = "map";
        chase_goal_[0].pose.orientation.x = 0;
        chase_goal_[0].pose.orientation.y = 0;
        chase_goal_[0].pose.orientation.z = 0;
        chase_goal_[0].pose.orientation.w = 1;
        chase_goal_[0].pose.position.x = 0;
        chase_goal_[0].pose.position.y = 0;
        chase_goal_[0].pose.position.z = 0;

        chase_goal_[1].header.frame_id = "map";
        chase_goal_[1].pose.orientation.x = 0;
        chase_goal_[1].pose.orientation.y = 0;
        chase_goal_[1].pose.orientation.z = 0;
        chase_goal_[1].pose.orientation.w = 1;
        chase_goal_[1].pose.position.x = 0;
        chase_goal_[1].pose.position.y = 0;
        chase_goal_[1].pose.position.z = 0;

        //chase_buffer_.resize(2);
        //chase_count_ = 0;

        whirl_vel_.accel.linear.x = 0;
        whirl_vel_.accel.linear.x = 0;
        whirl_vel_.accel.linear.y = 0;
        whirl_vel_.accel.linear.z = 0;
        whirl_vel_.accel.angular.x = 0;
        whirl_vel_.accel.angular.y = 0;
        whirl_vel_.accel.angular.z = 0;

        ROS_INFO("load file");
        LoadParam(proto_file_path);

      }

      ~Blackboard() = default;

      /*---------------------------------GET---------------------------------------*/

      bool IsMaster(){
        return is_master_;
      }
      // 从blackboard获取参数的函数
      geometry_msgs::PoseStamped GetEnemy() const {
        return enemy_pose_;
      }

      geometry_msgs::PoseStamped GetBuffPose(){
        return buff_pose_;
      }

      //unsigned int GetLastShootNum(){
        //return shoot_number_;
      //}

      //ShootMode GetShootMode(){
        //return shoot_mode_;
      //}

      geometry_msgs::PoseStamped GetRobotMapPose(){
        UpdateRobotPose();
        return robot_map_pose_;
      }

      //std::vector<geometry_msgs::PoseStamped> GetChaseBuffer(){
        //return chase_buffer_;
      //}

      //int GetChaseCount(){
        //return chase_count_;
      //}

      bool IsEnemyLost(){
        if(sentry_lost_ && camera_lost_ ){
          return true;
        } else{
          return false;
        }
      }

      geometry_msgs::PoseStamped GetEnemyPose(){
        // 初始化
        geometry_msgs::PoseStamped enemy_pose;
        enemy_pose.header.frame_id = "map";
        enemy_pose.pose.position.x = 0;
        enemy_pose.pose.position.y = 0;
        enemy_pose.pose.position.z = 0;
        tf::Quaternion quaternion = tf::createQuaternionFromRPY(0, 0, 0);
        enemy_pose.pose.orientation.x = quaternion.x();
        enemy_pose.pose.orientation.y = quaternion.y();
        enemy_pose.pose.orientation.z = quaternion.z();
        enemy_pose.pose.orientation.w = quaternion.w();
        try{
          enemy_pose = HandleEnemyPose();
        }
        catch(std::exception& e){
          ROS_WARN("handle enemy pose error : %s", e.what());
        }
        return enemy_pose_;
      }

      bool GetCancelFlag(){
        return cancel_flag_;
      }

      CostMap2D* GetCostMap2D(){
        return costmap_2d_;
      }

      bool IsGimbalView(){
        return is_scan_;
      }

      // 搜索行为中向目标趋向的累计                                             
      int GetSearchCount(){
        return search_count_;
      }
      
      int GetSearchPointSize(){
        return search_points_size_;
      }

      std::vector<geometry_msgs::PoseStamped> GetSearchRegion1(){
        return search_region_1_;
      }

      std::vector<geometry_msgs::PoseStamped> GetSearchRegion2(){
        return search_region_2_;
      }

      std::vector<geometry_msgs::PoseStamped> GetEscapePoints(){
        return escape_points_;
      }

      std::shared_ptr<ChassisExecutor> GetChassisExecutor(){
        return chassis_executor_ptr_;
      }

      std::shared_ptr<GimbalExecutor> GetGimbalExecutor(){
        return gimbal_executor_ptr_;
      }

      int GetRobotId(){
      	return id_;
      }

      BehaviorState GetActionState(){
        return action_state_;
      }

      //geometry_msgs::PoseStamped GetFriendPose(){
        //  UpdateFriendPose();
        //return friend_pose_;
      //}

      double GetShootingRange(){
        return shooting_range_;
      }

      bool IsChase(){
        if(GetEnemyDistance() < 2.0){         //-----------motify number
 	        is_chase_=false;
        }
        ROS_INFO("is chase:%d",is_chase_);
        return is_chase_;
      }

      bool IsFollow(){
      	ROS_INFO("is follow:%d",is_follow_);
	      return is_follow_;
      }

      bool IsEscape(){
	      ROS_INFO("is escape:%d",is_escape_);
	      return is_escape_;
      }

    /*  bool IsUnderPunish(){
        ROS_INFO("is under punishent:%d",under_punishment_);
        return under_punishment_;
      }*/

      bool IsSwing(){
        return is_swing_;
      }

      unsigned int GetRemainBullets(){
        ROS_INFO("remain_bullets_: %d",remain_bullet_);
        return remain_bullet_;
      }

      unsigned int GetRemainHP(){
        ROS_INFO("remain_hp_: %d",remain_hp_);
        return remain_hp_;
      }

      unsigned int GetCurrentHeat(){
        ROS_INFO("Current heat : %d",heat_);
        return heat_;
      }

      unsigned int GetTeammateBullets(){
        ROS_INFO("teammate_remain_bullets_: %d",teammate_bullet_);
        return teammate_bullet_;
      }

      unsigned int GetTeammateHP(){
        ROS_INFO("teammate_remain_hp_: %d",teammate_hp_);
        return teammate_hp_;
      }


      geometry_msgs::PoseStamped GetTeammatePose(){
        return teammate_pose_;     
      }

      // 自己是否受到惩罚
      bool IfPunished(){
        return if_punished_;
      }   

      // 队友是否受到惩罚
      bool IfTeammatePunished(){
        return mate_punished_;
      }

      bool GimbalPunished(){
        return gimbal_punished_;
      }

      bool ChassisPunished(){
        return chassis_punished_;
      }

      bool TeammateAttacked(){
        return mate_attacked_;
      }

      int GetGameStatus(){
        return game_status_;
      }
      //获取回血buff状态------------------------motify
      int GetBonusStatus(){
        if(GetRobotId()==3||GetRobotId()==4){ 
          return red_bonus_;
        }
        if(GetRobotId()==13||GetRobotId()==14){
          return blue_bonus_;
        }
        return 0;
      }

      //获取子弹buff状态------------------------motify 
      int GetSupplierStatus(){
        if(GetRobotId()==3||GetRobotId()==4){ 
          return red_supplier_;
        }
        if(GetRobotId()==13||GetRobotId()==14){
          return blue_supplier_;
        }
        return 0;
      }

      geometry_msgs::PoseStamped GetRobotGoal(){
        return robort_goal_;
      }

      bool IsRunning(){
        return is_running_;
      }

      _Status GetStatus(){
        return status_;
      }

      geometry_msgs::PoseStamped GetWaitRefreshPos(){
        return wait_refresh_position_;
      }

      int GetNearEnemyNUm(){
        return near_enemy_num_;
      } 

      int GetChaseEnemyNum(){
        return chase_enemy_num_;
      }

      std::vector<geometry_msgs::PoseStamped> GetChaseGoal(){
        return chase_goal_;
      }


      /*---------------------------------Set---------------------------------------*/

      void SetSwing(bool flag){
        is_swing_ = flag;
      }
      // 改变blackboard里面参数值的函数
      void SetEnemy(geometry_msgs::PoseStamped &enemy_pose){
        enemy_pose_=enemy_pose;
      }

      void SetBuffPose(geometry_msgs::PoseStamped &buff_pose){
        buff_pose_ = buff_pose;
      }

      //void SetShootMode(unsigned int &shoot_mode){
        //shoot_mode_ = shoot_mode;
      //}

      //void SetShootNum(unsigned int &shoot_number){
        //shoot_number_ = shoot_number;
      //}

      //void SetChaseCount(unsigned int &count){
        //chase_count_ = count;
      //}

      void CancelSearch() {
        SetSearchCount(0);
      }

      void SetEnemyLost(bool lost){
        enemy_lost_ = lost;
      }

      //void SetChaseBuffer(std::vector<geometry_msgs::PoseStamped> &buffer){
        //chase_buffer_ = buffer;
      //}

      void SetCancelFlag(bool flag){
        cancel_flag_ = flag;
      }

      void SetGimbalScan(bool is_scan){
        is_scan_ = is_scan;
      }

      void SetSearchCount(unsigned int count){
        ROS_INFO("set search count: %d",search_count_);
        search_count_ = count;
      }

      void SetRobotId(int id){
      	 id_ = id;
      }  
      
      void SetActionState(BehaviorState action_state){
        action_state_=action_state;
      }

      void SetFriendMapPose(geometry_msgs::PoseStamped friend_pose){
        teammate_pose_ = friend_pose;
      }

      void SetChase(bool is_chase){
        ROS_INFO("set chase:%d",is_chase);
        is_chase_ = is_chase;
      }

      void SetFollow(bool is_follow){
        ROS_INFO("set chase:%d",is_follow);
        is_follow_ = is_follow;
      }
      void SetBuffGot(bool has_got){
        has_got_ = has_got;
      }

      void SetEscape(bool is_escape){
        ROS_INFO("set escape:%d",is_escape);
        is_escape_=is_escape;
      }

      void SetShootingRange(double shooting_range){
        ROS_INFO("set shooting range:%d",shooting_range);
        shooting_range_ = shooting_range;
      }

    /*  void SetUnderPunish(bool under_punishment){
        ROS_INFO("set under punishment:%d",under_punishment);
        under_punishment_ = under_punishment;
      }*/

      void SetGimbalView(bool is_scan){
        is_scan_ = is_scan;
        //gimcontrol.decision_control = is_scan_;
      }

      void SetRobotGoal(geometry_msgs::PoseStamped robort_goal){
        robort_goal_ = robort_goal;
      }

      void SetNearEnemyNum(int num){
        near_enemy_num_ = num;
      }

      void SetChaseEnemyNUm(int num){
        chase_enemy_num_ = num;
      }



      /*-------------------------------功能区---------------------------------------*/


      // 控制摩擦轮开启
      /*bool SetFricWhl()
      {
        if (fricwhl_)
            return true;                // 如果已经开启,则直接返回
        roborts_msgs::FricWhl fric_msg; // 控制摩擦轮的服务消息
        fric_msg.request.open = true;   // 开启
        if (fricwhl_client_.call(fric_msg) && fric_msg.response){
            fricwhl_ = true;
            return true;
        }// 如果成功,返回true
        else{
          ROS_INFO("Fric set erorr .");
          fricwhl_ = false;
          return false;
        } //如果开启异常,返回false
      }     //set fric

      bool OffFricWhl()
      {
        if (!fricwhl_)
          return true;                            // 如果已经关闭,则直接返回
        roborts_msgs::FricWhl fric_msg;           // 控制摩擦轮的服务消息
        fric_msg.request.open = false;            // 关闭
        if (fricwhl_client_.call(fric_msg) && fric_msg.response){
          fricwhl_ = false;
          return true;
        } // 如果成功,返回true
        else{
          fricwhl_ = true;
          ROS_INFO("Fric off erorr .");
          return false;
        } //如果关闭异常,返回false
      }     //off fric*/

      // 判断是否转向
      bool IfTurn(){
        if(damage_type_ == 0){
              return true;
        }else{
          return false;
        }
      }


      //受到攻击时得到转向角---------------------------waiting motify
      int  ArmorDamageSource(){
        auto turn_time = (ros::Time::now()-last_armor_attack_time_).toSec();
        auto median = damage_source_last_;
        damage_source_last_ = damage_source_;
        if(turn_time < 0.5 ){
          if(median == damage_source_){
            return damage_source_;
          }
          if((median==1 && damage_source_==3)||(median==3 && damage_source_==1)){
            if(GetEnemyYaw()>0){
              return 1;
            }else{
              return 3;
            }
          }
          if(damage_source_!=2 &&median!= 2){
            return 0;
          }
          if(damage_source_==2){
            return median;
          }
          if(median==2){
            return median;
          }
        }else{
          return 0;
        }
      }

      void UpdateRobotPose(){
        // 设置机器人当前位置，存储在robot_map_pose_中
         /*
        获取坐标的原理:
            base_link 是 map 中的一个子坐标系, 声明一个点, 默认为原点
            此时base_link在map中的坐标, 就是base_link原点在map中的坐标
        */
        tf::Stamped<tf::Pose> robot_tf_pose;
        robot_tf_pose.setIdentity();
        robot_tf_pose.frame_id_ = "base_link";  // 以上声明一个base_link中的位置变量,默认原点
        robot_tf_pose.stamp_ = ros::Time();
        try{
            geometry_msgs::PoseStamped robot_pose;
            tf::poseStampedTFToMsg(robot_tf_pose, robot_pose);
            // 从 base_link 转换到 map
            tf_ptr_->transformPose("map", robot_pose, robot_map_pose_);
        }
        catch(tf::LookupException& e){
            ROS_ERROR("Transform Error looking up robot pose: %s", e.what());
        }
      }

      geometry_msgs::PoseStamped HandleEnemyPose(){
        // 待完成
        // 根据哨岗传回的两个坐标还有目前本机是主机还是僚机分析一下，返回一个敌人坐标
        auto robot_map_pose = GetRobotMapPose();                                  
        auto dx = robot_map_pose.pose.position.x - enemy_pose1_.pose.position.x;        
        auto dy = robot_map_pose.pose.position.y - enemy_pose1_.pose.position.y; 
        double dis_one = std::sqrt(std::pow(dx, 2) + std::pow(dy, 2));
        auto dp = robot_map_pose.pose.position.x - enemy_pose2_.pose.position.x;        
        auto dq = robot_map_pose.pose.position.y - enemy_pose2_.pose.position.y; 
        double dis_two = std::sqrt(std::pow(dp, 2) + std::pow(dq, 2));
        if(dis_one<=dis_two){
          SetNearEnemyNum(1);    //距自己近的敌人为敌方一号车
          return enemy_pose1_;
        }else{
          SetNearEnemyNum(2);
          return enemy_pose2_;
        }
      }

      double GetEnemyYaw(){
        auto robot = GetRobotMapPose();             
        auto enemy = GetEnemyPose();
        tf::Quaternion rot1, rot2;
        tf::quaternionMsgToTF(robot.pose.orientation, rot1);
        tf::quaternionMsgToTF(enemy.pose.orientation, rot2);
        escape_yaw_ =  rot1.angleShortestPath(rot2);
        return escape_yaw_;
      }

      // 通过目前机器人的坐标, 判断机器人处于哪一个区域
      int GetCurrentReg(geometry_msgs::PoseStamped pose){
        // FIXME 通过机器人坐标,判断机器人目前处于什么区域
        //      用对角线划分为四个区域
        //已改
        double x, y;
        x = pose.pose.position.x;
        y = pose.pose.position.y;
        if(x <= 4.04){
          if(y > 2.24){
            return 0;
          }else
          {
            return 3;
          }
        }else{
          if(y > 2.4){
            return 1;
          }else{
            return 2;
          }
        }

        /*if(y > 0.625 * x){
          if(y < -0.625*x + 5.0){
              return 1;
          }
          else
              return 2;
        }
        else{
          if(y < -0.625*x + 5.0)
              return 3;
          else 
              return 4;
        }*/
      }

      //void LoadParam(){
        // 加载参数, 参数来源, prototxt文件
        // 参数包含:
        //      逃跑路线
        //      搜索路线
        //      buff的坐标
        //      起始位置, 包括主机和僚机
        //      主机还是僚机
        //      仿真还是实战
        //      
    //}
      
      void LoadParam(const std::string &proto_file_path) {
        //roborts_decision::DecisionConfig decision_config;
        if (!roborts_common::ReadProtoFromTextFile(proto_file_path, &decision_config)) {
            ROS_ERROR("Load param failed !");
            return ;
        }
        ///-------------------boot----------------------------
        if(IsMaster()){
          for (int i = 0; i != decision_config.master_bot().size(); i++) {
            geometry_msgs::PoseStamped master_boot;
            master_boot.header.frame_id = "map";
            master_boot.pose.position.x = decision_config.master_bot(i).x();
            master_boot.pose.position.z = decision_config.master_bot(i).z();
            master_boot.pose.position.y = decision_config.master_bot(i).y();
            auto master_quaternion = tf::createQuaternionMsgFromRollPitchYaw(decision_config.master_bot(i).roll(),
                                                                          decision_config.master_bot(i).pitch(),
                                                                          decision_config.master_bot(i).yaw());
            master_boot.pose.orientation = master_quaternion;
            boot_position_.push_back(master_boot);
          }
        }else{
          for (int i = 0; i != decision_config.auxe_bot().size(); i++) {
            geometry_msgs::PoseStamped auxe_boot;
            auxe_boot.header.frame_id = "map";
            auxe_boot.pose.position.x = decision_config.auxe_bot(i).x();
            auxe_boot.pose.position.z = decision_config.auxe_bot(i).z();
            auxe_boot.pose.position.y = decision_config.auxe_bot(i).y();
            auto auxe_quaternion = tf::createQuaternionMsgFromRollPitchYaw(decision_config.auxe_bot(i).roll(),
                                                                          decision_config.auxe_bot(i).pitch(),
                                                                          decision_config.auxe_bot(i).yaw());
            auxe_boot.pose.orientation = auxe_quaternion;
            boot_position_.push_back(auxe_boot);
          }
        }
    
        ///-------------------buff----------------------------
        buff_size_ = decision_config.buff_point().size();
        fixed_buff_.resize(buff_size_);
        for (int i = 0; i != buff_size_; i++) {
            fixed_buff_[i].header.frame_id = "map";
            fixed_buff_[i].pose.position.x = decision_config.buff_point(i).x();
            fixed_buff_[i].pose.position.y = decision_config.buff_point(i).y();
            fixed_buff_[i].pose.position.z = decision_config.buff_point(i).z();

            tf::Quaternion quaternion = tf::createQuaternionFromRPY(decision_config.buff_point(i).roll(),
                                                                    decision_config.buff_point(i).pitch(),
                                                                    decision_config.buff_point(i).yaw());
            fixed_buff_[i].pose.orientation.x = quaternion.x();
            fixed_buff_[i].pose.orientation.y = quaternion.y();
            fixed_buff_[i].pose.orientation.z = quaternion.z();
            fixed_buff_[i].pose.orientation.w = quaternion.w();
        }

        ///----------------wait for refresh position-------------------
        wait_refresh_size_ = decision_config.buff_point().size();
        wait_refresh_pos_.resize(wait_refresh_size_);
        for (int i = 0; i != wait_refresh_size_; i++) {
            wait_refresh_pos_[i].header.frame_id = "map";
            wait_refresh_pos_[i].pose.position.x = decision_config.wait_point(i).x();
            wait_refresh_pos_[i].pose.position.y = decision_config.wait_point(i).y();
            wait_refresh_pos_[i].pose.position.z = decision_config.wait_point(i).z();

            tf::Quaternion quaternion = tf::createQuaternionFromRPY(decision_config.wait_point(i).roll(),
                                                                    decision_config.wait_point(i).pitch(),
                                                                    decision_config.wait_point(i).yaw());
            wait_refresh_pos_[i].pose.orientation.x = quaternion.x();
            wait_refresh_pos_[i].pose.orientation.y = quaternion.y();
            wait_refresh_pos_[i].pose.orientation.z = quaternion.z();
            wait_refresh_pos_[i].pose.orientation.w = quaternion.w();
        }

        ///-------------------search--------------------------
        search_points_size_ = decision_config.search_path_1().size();
        search_region_1_.resize((unsigned int)(decision_config.search_path_1().size()));
        for (int i = 0; i != decision_config.search_path_1().size(); i++) {
            geometry_msgs::PoseStamped search_point;
            search_point.header.frame_id = "map";
            search_point.pose.position.x = decision_config.search_path_1(i).x();
            search_point.pose.position.y = decision_config.search_path_1(i).y();
            search_point.pose.position.z = decision_config.search_path_1(i).z();

            auto quaternion = tf::createQuaternionMsgFromRollPitchYaw(decision_config.search_path_1(i).roll(),
                                                                      decision_config.search_path_1(i).pitch(),
                                                                      decision_config.search_path_1(i).yaw());
            search_point.pose.orientation = quaternion;
            search_region_1_.push_back(search_point);
        }
        search_region_2_.resize((unsigned int)(decision_config.search_path_2().size()));
        for (int i = 3; i != decision_config.search_path_2().size(); i++) {
            geometry_msgs::PoseStamped search_point;
            search_point.header.frame_id = "map";
            search_point.pose.position.x = decision_config.search_path_2(i).x();
            search_point.pose.position.y = decision_config.search_path_2(i).y();
            search_point.pose.position.z = decision_config.search_path_2(i).z();

            auto quaternion = tf::createQuaternionMsgFromRollPitchYaw(decision_config.search_path_2(i).roll(),
                                                                      decision_config.search_path_2(i).pitch(),
                                                                      decision_config.search_path_2(i).yaw());
            search_point.pose.orientation = quaternion;
            search_region_2_.push_back(search_point);
        }

        ///-----------------------------escape-----------------------------------
        escape_points_.resize((unsigned int)(decision_config.escape().size()));
        for (int i = 0; i != decision_config.escape().size(); i++) {
            geometry_msgs::PoseStamped escape_point;
            escape_point.header.frame_id = "map";
            escape_point.pose.position.x = decision_config.escape(i).x();
            escape_point.pose.position.y = decision_config.escape(i).y();
            escape_point.pose.position.z = decision_config.escape(i).z();

            auto quaternion = tf::createQuaternionMsgFromRollPitchYaw(decision_config.escape(i).roll(),
                                                                      decision_config.escape(i).pitch(),
                                                                      decision_config.escape(i).yaw());
            escape_point.pose.orientation = quaternion;
            escape_points_.push_back(escape_point);
        }



        whirl_vel_.twist.angular.z = decision_config.whirl_vel().angle_z_vel(); //default == 1
        whirl_vel_.twist.angular.y = decision_config.whirl_vel().angle_y_vel(); //        == 0
        whirl_vel_.twist.angular.x = decision_config.whirl_vel().angle_x_vel(); //        == 0

      }

      //判断是否拿到了buff
      bool IfGotBuff(){
        return has_got_;
      }

      
      // 判断是否马上要刷新buff区状态
      bool IsNearRefresh(){
        // remaining_time_ = static_cast<unsigned int>(game_info->remaining_time);
        if((remaining_time_ - 60 >=0) && (remaining_time_ - 60 <=1.5)){
          return true;
        }
        if((remaining_time_ - 120 >=0) && (remaining_time_ - 120 <=1.5)){
          return true;
        } else{
          return false;
          }
      }



      //判断需要哪种buff，并set相应的buffpose

      //判断需要哪种buff，并获取相应的buffpose
      // FIXME buff位置的获取是否有问题
      //见868
      void BuffPose(int kind_of_buff_){
        if(kind_of_buff_==0){
          SetBuffPose(random_buff_[0]);  //blood
        }
        if(kind_of_buff_==1){
          SetBuffPose(random_buff_[1]);  //bullets
        }else{
           ROS_INFO("can't gain buff pose!");  
        }
      }
 
      //0是1号车，1是2号车
      //int RobotId(){
        //return GetRobotId()++ % 2;
      //}

      //获得与敌人的距离
      double GetEnemyDistance(){
        auto robot_map_pose = GetRobotMapPose();             
        auto enemy_map_pose = GetEnemyPose();                                  
        auto dx = robot_map_pose.pose.position.x - enemy_map_pose.pose.position.x;        
        auto dy = robot_map_pose.pose.position.y - enemy_map_pose.pose.position.y;        
        double distance = std::sqrt(std::pow(dx, 2) + std::pow(dy, 2));
        return distance;    
      }

      geometry_msgs::PoseStamped LoadBootPosition(){
        int num = 0;
        if(GetRobotId()>10){         //小于10蓝方，大于10红方
          num = 1;
        }
        return boot_position_[num];
      }

      //上次受到攻击的时刻
      ros::Time GetLastArmorAttackTime(){
        ROS_INFO("Last_Armor_attacker_time:%lf",last_armor_attack_time_.toSec());     
        ROS_INFO("Now_time: %lf",ros::Time::now().toSec());
        ROS_INFO("Time_difference: %lf",(ros::Time::now()-last_armor_attack_time_).toSec()); 
        return last_armor_attack_time_;
      }
      
      //若自己距buff更近，则返回true;若队友距buff更近则返回false
      // FIXME buff_pose的获得好像有问题  865，868
      //已改，fixed_buff_表示六个从配置文件中获取的固定位置，用来判断快刷新时，机器人是否在这六个位置附近
      //random_buff_表示每次随机刷新的我方两种buff区位置，从裁判系统获得
      bool NearBuff(){
        geometry_msgs::PoseStamped buff_pose;
        if(remain_hp_<=1000 || teammate_hp_<=1000){
          buff_pose = random_buff_[0];
        }
        if(remain_bullet_<=20 || teammate_bullet_<=20){
          buff_pose = random_buff_[1];
        }else{
          ROS_INFO("don't need buff!");  
          return false;
        }
        auto robot_map_pose = GetRobotMapPose();             
        auto teammate_pose = GetTeammatePose();                                  
        auto dx = robot_map_pose.pose.position.x - buff_pose.pose.position.x;        
        auto dy = robot_map_pose.pose.position.y - buff_pose.pose.position.y; 
        double dis_self = std::sqrt(std::pow(dx, 2) + std::pow(dy, 2));
        auto dp = teammate_pose.pose.position.x - buff_pose.pose.position.x;        
        auto dq = teammate_pose.pose.position.y - buff_pose.pose.position.y; 
        double dis_mate = std::sqrt(std::pow(dp, 2) + std::pow(dq, 2));
        if(dis_self<=dis_mate){
          return true;
        }else{
          return false;
        }
      }

      //每秒掉血量---------------------------time_diff--waiting moyify
      double HurtedPerSecond() {
        auto reduce_hp_ = last_hp_ - remain_hp_;         
        auto time_diff = (ros::Time::now()-last_check_attacked_time_).toSec();
        if (time_diff > 0.5) {  
          // FIXME 这个类型转换有问题    
          //已改            
          self_dmp_ = reduce_hp_ / time_diff;
          last_hp_ = remain_hp_;
          last_check_attacked_time_ = ros::Time::now();
          ROS_INFO("meet_if_condition_last_hp: %d",last_hp_);
          ROS_INFO("meet_if_condition_remain_hp: %d",remain_hp_);
          ROS_INFO("meet_if_condition_reduce_hp: %d",reduce_hp_);
          ROS_INFO("meet_if_condition_timeDiff: %f",time_diff);
          ROS_INFO("meet_if_condition_dmp: %lf",self_dmp_);
          return self_dmp_;
        } else {
            ROS_INFO("meet_else_condition_last_hp: %d",last_hp_);
            ROS_INFO("meet_else_condition_remain_hp: %d",remain_hp_);
            ROS_INFO("meet_else_condition_reduce_hp: %d",reduce_hp_);
            ROS_INFO("meet_else_condition_timeDiff: %f",time_diff);
            // FIXME 这个self_dmp_的值是什么
            //是上一次的计算值
            ROS_INFO("meet_else_condition_dmp: %lf",self_dmp_);
            return self_dmp_;
          }
      }

      //每秒敌人掉血量---------------------------time_diff--waiting moyify
      double HurtPerSecond() {
        auto reduce_hp_ = last_enemy_hp_ - enemy_hp_;         
        auto time_diff = (ros::Time::now()-last_check_attack_time_).toSec();
        if (time_diff > 0.5) {
          enemy_dmp_ =int( reduce_hp_) / time_diff;
          last_enemy_hp_ = enemy_hp_;
          last_check_attack_time_ = ros::Time::now();
          ROS_INFO("meet_if_condition_enemy_last_hp: %d",last_hp_);
          ROS_INFO("meet_if_condition_enemy_remain_hp: %d",remain_hp_);
          ROS_INFO("meet_if_condition_enemy_reduce_hp: %d",reduce_hp_);
          ROS_INFO("meet_if_condition_enemy_timeDiff: %f",time_diff);
          ROS_INFO("meet_if_condition_enemy_dmp: %lf",enemy_dmp_);
          return enemy_dmp_;
        } else {
            ROS_INFO("meet_else_condition_enemy_last_hp: %d",last_hp_);
            ROS_INFO("meet_else_condition_enemy_remain_hp: %d",remain_hp_);
            ROS_INFO("meet_else_condition_enemy_reduce_hp: %d",reduce_hp_);
            ROS_INFO("meet_else_condition_enemy_timeDiff: %f",time_diff);
            ROS_INFO("meet_else_condition_enemy_dmp: %lf",enemy_dmp_);
            return enemy_dmp_;
          }
      }      

      double WithAdvantage(){
        if(HurtedPerSecond()<=0 || HurtPerSecond()<=0){
          ROS_INFO("dmp_ <= 0! No war ?");
          return -10;
        }
        double self_over = remain_hp_ / HurtedPerSecond();
        double enemy_over = enemy_hp_ / HurtPerSecond();
        return enemy_over - self_over;
      }

      bool OnBuffLocation(){
        for(int i=0;i<6;i++){
          auto robot_map_pose = GetRobotMapPose();             
          auto special_pose = fixed_buff_[i];                                  
          auto dx = robot_map_pose.pose.position.x - special_pose.pose.position.x;        
          auto dy = robot_map_pose.pose.position.y - special_pose.pose.position.y;        
          double distance = std::sqrt(std::pow(dx, 2) + std::pow(dy, 2));
          if(distance<=0.67){
            wait_refresh_position_ = wait_refresh_pos_[i];
            return true;
          }
        }
        return false;
      }

      void UpdateGimcontrol(){
        gimcontrol.decision_control = is_scan_;
      }

      void UpdateShootPermit(){
        gimcontrol.can_shoot = !gimbal_punished_;
      }

      // FIXME 这个函数执行了吗，每次决策的时候都应该执行一次
      //执行了，在主决策节点中调用
      void PublishGimcontrol(){
        UpdateGimcontrol();
        UpdateShootPermit();
        gim_control_pub.publish(gimcontrol);
      }



      /*---------------------------回调函数--------------------------------*/
      //interact callback
      void MateInfoCallBack(const roborts_msgs::RobotInfo::ConstPtr & mate_info){
        teammate_hp_ = static_cast<unsigned int>(mate_info->Hp);
        teammate_bullet_ = static_cast<unsigned int>(mate_info->Bullet);
        teammate_heat_ = static_cast<unsigned int>(mate_info->Heat);
        teammate_pose_ = mate_info->RobotPose;
      }

      void MatePubnishCallBack(const roborts_msgs::PunishInfo::ConstPtr & punish_info){
        mate_gimbal_ = punish_info->on_gimbal;
        mate_chassis_ = punish_info->on_chassis;
        if(mate_gimbal_ || mate_chassis_){
          mate_punished_ = true;
        }else{
          mate_punished_ = false;
        }
      }

      void MateTreeStatusCallBack(const roborts_msgs::TreeStatus::ConstPtr & tree_status){
        mate_status_=static_cast<unsigned int>(tree_status->status); 
        mate_tree_running_=tree_status->is_running;
        mate_goal_=tree_status->goal; 
      }

      //enemy pose callback
      void EnemyPoseCallBack(const roborts_msgs::SentryInfo::ConstPtr & sentry_info){
        sentry_lost_ = sentry_info->can_view;
        if(!sentry_lost_){
          ROS_INFO("recieve enemy pose from sentry!");
          enemy_pose1_.pose.position.x =sentry_info->x1;
          enemy_pose1_.pose.position.y =sentry_info->y1;
          enemy_pose2_.pose.position.x =sentry_info->x2;
          enemy_pose2_.pose.position.y =sentry_info->y2;
          chase_goal_[0].pose.position.x = enemy_pose1_.pose.position.x;
          chase_goal_[0].pose.position.y = enemy_pose1_.pose.position.y;
          chase_goal_[1].pose.position.x = enemy_pose1_.pose.position.x;
          chase_goal_[1].pose.position.y = enemy_pose1_.pose.position.y;
        }else{
          ROS_INFO("can't get enemy pose from sentry !");
        }
      }

      void CameraCallBack(const roborts_msgs::PyArmorInfo::ConstPtr & camera_info){
        camera_lost_ = camera_info->is_enemy;     
        if(!camera_lost_){
          ROS_INFO("recieve enemy distance from camera !");
          camera_dis_ = static_cast<double>(camera_info->z);                
        cls_ = static_cast<int>(camera_info->cls); 
        }else{
          ROS_INFO("can't get enemy distance from camera !");
        }
      }

      //refree callback
      //void BuffStatusCallBack(/*消息类型*/){
        // 根据裁判系统提供的buff区的信息，处理各个buff位置
        // 根据情况设置当前应当去获取的buff的位置
      //}

      //void SupplierStatusCallback(){
        //supplier_status 

        // remain_bullet_
        //}
      //}

      //void RobotShootCallback(){

      // remain_bullet_--;
      //}

      //void RobotDamageCallback(){
        //last_armor_attack_time_
      //}
  

  private:

      std::vector<geometry_msgs::PoseStamped> boot_position_;    //出发地位置
      
      bool is_follow_;                              //是否跟随
      
      bool has_got_;                                //是否拿到了buff
      unsigned int red_bonus_;                      //红方回血buff状态
      unsigned int blue_bonus_;                     //蓝方回血buff状态
      unsigned int red_supplier_;                   //红方子弹buff状态
      unsigned int blue_supplier_;                  //蓝方子弹buff状态
      geometry_msgs::PoseStamped buff_pose_;        //buff区位置
    //  bool under_punishment_;                       //是否在惩罚中

      int id_;                                      //机器人编号
      bool is_master_;                              //是否主机        
      geometry_msgs::PoseStamped teammate_pose_;      //队友机器人位置


      BehaviorState action_state_;                  //动作节点的状态

      std::vector<geometry_msgs::PoseStamped> chase_goal_;       //追击目标坐标
      bool is_chase_;                               //是否正在追击
      //std::vector<geometry_msgs::PoseStamped> chase_buffer_;
      //unsigned int chase_count_;
      unsigned int near_enemy_num_;                 //距离更近的敌人编号
      unsigned int chase_enemy_num_;                 //正在追击的敌人编号

      geometry_msgs::PoseStamped enemy_pose_;       //现在正在攻击的敌人位置     
      //ShootMode shoot_mode_;                        //子弹发射的模式            
      //unsigned int shoot_number_;                   //子弹发射数量              
      double shooting_range_;
      //std::vector<geometry_msgs::PoseStamped> blood_buff_;          //当前blood buff的位置
      //std::vector<geometry_msgs::PoseStamped> bullets_buff_;        //当前bullets buff的位置
      std::vector<geometry_msgs::PoseStamped> fixed_buff_;   //六个惩罚加成区位置
      std::vector<geometry_msgs::PoseStamped> random_buff_;  //两个随机的加成区
      geometry_msgs::PoseStamped wait_refresh_position_;     //等待刷新位置
      std::vector<geometry_msgs::PoseStamped> wait_refresh_pos_; //等待刷新位置

      unsigned int remaining_time_;                 //游戏剩余时间，由裁判系统的游戏信息获得

      geometry_msgs::PoseStamped robot_map_pose_;   // 机器人当前位置
      geometry_msgs::PoseStamped robort_goal_;      // 下一步目标位置
      //std::vector<geometry_msgs::PoseStamped> chase_buff_;  

      bool enemy_lost_;                             // 追击时敌人是否观察不到
      bool sentry_lost_;                            // 是否与哨岗失去联系
      geometry_msgs::PoseStamped enemy_pose1_;      //敌方一号车
      geometry_msgs::PoseStamped enemy_pose2_;      // 敌方二号车
      bool cancel_flag_;                            // 是否需要采取取消行为
      CostMap2D* cost_map_2d_;

      bool is_swing_;

      unsigned int search_count_;
      unsigned int search_points_size_;
      std::vector<geometry_msgs::PoseStamped> search_region_1_;
      std::vector<geometry_msgs::PoseStamped> search_region_2_;       // 两个搜索路径，由外部参数加载

      std::vector<geometry_msgs::PoseStamped> escape_points_;         // 撤离目标
      bool is_escape_;                                                //是否逃跑
      double escape_yaw_;                                             //逃跑时转向角

      std::shared_ptr<tf::TransformListener> tf_ptr_;                 // 声明tf树 
      bool is_running_;
      _Status status_;

      int damage_source_;            //伤害源位置
      int damage_source_last_;        //上次伤害源位置
      int damage_source_now_;        //此次伤害源位置
      int damage_type_;              //伤害类型
      bool mate_attacked_;           //队友是否受到攻击
      ros::Time last_armor_attack_time_;          //上次受到攻击的时刻

      ros::Time last_check_attacked_time_;        //上次检测自己剩余血量的时刻
      ros::Time last_check_attack_time_;          //上次检测敌人剩余血量的时刻
      unsigned int remain_bullet_;      //自己剩余子弹量
      unsigned int remain_hp_;          //自己剩余血量
      unsigned int enemy_hp_;           //敌人剩余血量----------------是否可知？
      unsigned int last_hp_;               //上一次检测自己剩余血量
      unsigned int last_enemy_hp_;         //上一次检测敌人的剩余血量
      double self_dmp_;                 //自己每秒掉血量
      double enemy_dmp_;                //敌人每秒掉血量

      unsigned int heat_;           //自己枪口热量

      //bool first_action_;         //是否执行开场动作
      bool if_punished_;            //自己是否受到惩罚
      bool mate_punished_;          //队友是否受罚
      bool gimbal_punished_;        //自己云台是否受限
      bool chassis_punished_;       //自己底盘是否受限
      bool mate_gimbal_;            //队友云台是否受限
      bool mate_chassis_;           //队友底盘是否受限
      bool is_scan_;                     // 是否进行旋转扫描

      unsigned short buff_size_;       //loadparam
      unsigned short supplier_size_;   //loadparam
      unsigned short wait_refresh_size_;       //loadparam
         
      //cost map
      std::shared_ptr<CostMap> costmap_ptr_;
      CostMap2D* costmap_2d_;
      unsigned char* charmap_;

      //decision config
      roborts_decision::DecisionConfig decision_config;

      roborts_msgs::TwistAccel whirl_vel_;

      //executor
      std::shared_ptr<ChassisExecutor> chassis_executor_ptr_;
      std::shared_ptr<GimbalExecutor>  gimbal_executor_ptr_;
      //interact info
      unsigned int teammate_hp_;        //队友剩余血量
      unsigned int teammate_bullet_;    //队友剩余子弹量
      unsigned int teammate_heat_;      //队友枪口热量
      unsigned int mate_status_;        //1-追逐，2-逃跑，3-射击，4-找buff
      bool mate_tree_running_;           //队友行为树是否在运行
      geometry_msgs::PoseStamped mate_goal_;  //队友目标位置

      //camera info
      bool camera_lost_;                 //敌人是否不在视野内
      double camera_dis_;                //摄像头测量的与敌人的距离
      unsigned int cls_;                 //摄像头检测的装甲版方位

      //refree info
      unsigned int game_status_;        

      //gimbal publisher
      ros::Publisher gim_control_pub;
      roborts_msgs::GimbalControl gimcontrol;  

      //interact subscriber
      ros::Subscriber mate_info_sub;
      ros::Subscriber mate_punish_sub;
      ros::Subscriber mate_treestatus_sub;

      //sentry subscriber
      ros::Subscriber enemy_pose_sub;

      //camera subscriber
      ros::Subscriber camera_sub;
      //refree subscriber

  };// blackboard

}// robort_decision

#endif
// ROBORTS_DECISION_BLACKBOARD_H
